from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # 声明 launch 参数
    declare_map_frame = DeclareLaunchArgument(
        'map_frame',
        default_value='map',
        description='Map frame ID'
    )
    
    declare_base_frame = DeclareLaunchArgument(
        'base_frame',
        default_value='base_link',
        description='Base frame ID'
    )

    declare_map_topo_name = DeclareLaunchArgument(
        'map_topo_name',
        default_value='map_topo',
        description='Base frame ID'
    )

    declare_config_dir = DeclareLaunchArgument(
        'config_dir',
        default_value='/home/yelume/01-program/07-HandBot_SLAM/slam_ws/src/topo_nav/config',
        description='Directory containing topology configuration files'
    )
    
    declare_pcd_dir = DeclareLaunchArgument(
        'pcd_dir',
        default_value='/home/yelume/01-program/07-HandBot_SLAM/slam_ws/top_graph',
        description='Directory containing PCD map files'
    )
    
    
    # 创建拓扑导航节点
    topo_nav_node = Node(
        package='topo_nav',
        executable='topo_nav_node',
        name='topo_nav_node',
        output='screen',
        parameters=[{
            'map_frame': LaunchConfiguration('map_frame'),
            'base_frame': LaunchConfiguration('base_frame'),
            'config_dir': LaunchConfiguration('config_dir'),
            'pcd_dir': LaunchConfiguration('pcd_dir'),
            'map_topo_name': LaunchConfiguration('map_topo_name'),
        }],
        remappings=[
            ('/goal_pose', '/goal_pose'),
            ('/topo_nav_path', '/topo_nav_path'),
        ]
    )
    rviz_config = PathJoinSubstitution([FindPackageShare('topo_nav'), 'rviz', 'topo.rviz'])

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', rviz_config],
    )

    return LaunchDescription([
        declare_map_frame,
        declare_base_frame,
        declare_config_dir,
        declare_pcd_dir,
        declare_map_topo_name,
        topo_nav_node,
        rviz_node,
    ])
